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ABSTRACT 


The  kinematic  model  for  the  Advanced  Deburring  and  Chamfering  System  (ADACS)  robot,  which 
determines  the  position  and  orientation  of  the  manipulator  end-effector  for  a given  set  of  joint  angles,  is  developed 
using  the  standardized  Denavit-Hartenberg  method  as  well  as  an  alternative  link  transform  method.  The  Denavit- 
Hartenberg  notation  is  described  and  the  parameters  for  each  link  are  provided  in  table  form.  The  transform  matrix 
for  each  link  is  then  derived  using  these  parameters  and  the  mathematical  kinematic  model  is  developed.  The 
kinematic  model  has  the  form 


T6=T?T2T3fT3T4T5T| 


where  Te  is  a 4 x 4 matrix  representation  of  the  position  and  orientation  of  the  last  frame  with  respect  to  the  base 
frame. 


The  inverse  kinematic  model,  which  determines  the  joint  value  set  for  a given  end-effector  position  and 
orientation,  is  developed  from  the  kinematic  model  derived  using  the  Denavit-Hartenberg  method.  The  first  three 
joints  are  solved  using  a geometric  approach.  The  last  three  joints  are  solved  for  by  algebraic  and  trigonometric 
manipulation  of  the  rotation  part  of  the  transformation  matrix. 

There  are  two  problems  that  are  dealt  with  when  the  inverse  kinematic  model  is  solved  for.  One  is  the 
presence  of  a singularity  point  in  the  manipulator.  A singularity  occurs  when  two  or  more  joint  axes  line  up 
causing  an  infinite  number  of  possible  solutions  for  any  given  orientation.  As  the  singularity  is  approached, 
excessive  speed  occurs  in  joint  4 as  the  wrist  “rolls  over.”  A singularity  occurs  when  joint  5 is  zero.  When  a 
singularity  occurs,  joint  5 is  set  to  0,  joint  4 is  set  to  its  previous  value  and  joint  6 is  solved  for.  There  is  also  an 
ambiguity  in  the  wrist.  There  are  two  solutions  for  the  last  three  joints  for  a specific  orientation.  This  ambiguity  is 
referred  to  as  “wrist-flip”  and  “wrist-no-flip.”  Because  of  the  wrist  ambiguity,  there  are  two  possible  joint  sets  for  a 
given  position  and  orientation.  The  values  of  joints  1,  2 and  3 are  constant  for  both  sets,  while  there  are  two 
possibilities  for  joints  4,  5 and  6. 


1.  INTRODUCTION 


1.1  ADACS  Overview 

Research  into  automating  the  deburring  process  has  been  conducted  at  the  National  Institute  of  Standards  and 
Technology  (NIST)  since  1983.  Research  started  with  the  Cleaning  and  Deburring  Workstation  (CDWS)  which 
robotically  finished  parts  made  of  soft  metals  (aluminum  and  brass). 

A second  generation  deburring  workcell  is  currently  being  developed  at  NIST  under  a Cooperative  Research 
and  Development  Agreement  (CRDA)  with  the  U.S.  Navy  and  United  Technologies  Research  Center  (UTRC).  This 
Advanced  Deburring  and  Chamfering  System  (ADACS)  incorporates  a six  degree  of  freedom  Cincinnati  Milacron 
T^-646*  electric  robot  as  the  macromanipulator  for  an  actively  compliant  deburring  tool  which  serves  as  the 
micromanipulator  for  the  system.  ADACS  is  capable  of  deburring  and  chamfering  aerospace  parts  (engine  hubs  and 
turbine  blades)  made  from  high-strength  alloys  such  as  inconel.  UTRC  is  modeling  the  deburring  and  chamfering 
process  for  high-strength  alloys.  The  Navy  is  looking  for  an  automated  system  capable  of  deburring  and  chamfering 
these  intricate  engine  hubs  and  turbine  blades  manufactured  from  high-strength  alloys. 

A robot  was  chosen  over  an  NC  machine  for  several  reasons.  To  get  the  necessary  manipulations  for 
deburring  small  complicated  parts,  at  least  a five  degree  of  freedom  machine  is  required.  Five  degree  of  freedom  NC 
machines  are  aveiilable,  but  at  a much  greater  expense  than  a five  or  six  degree  of  freedom  robot  NC  machines  are 
also  designed  to  deal  with  larger  chip  removal.  In  deburring  of  hard  metals,  very  fine  metal  chips  are  produced.  The 
shields  and  protective  boots  on  the  NC  machine  would  not  prevent  these  small  chips  from  getting  into  the  machine 
workings  and  causing  damage. 

1.2  Description  of  T^-646  Robot* 

The  Cincinnati  Milacron  T^-646  is  a six  degree  of  freedom  electric  robot  with  a three  roll  wrist.  The 
position  of  the  end-effector  is  determined  by  the  angles  of  the  first  three  joints  and  the  orientation  by  the  three 
intersecting  joints  of  the  wrist. 

1.3  Kinematic  Model 

A manipulator  arm  can  be  described  as  a series  of  rigid  bodies  joined  together  in  a kinematic  structure.  This 
linkage,  constructed  with  a serial  or  “open  loop”  structure,  is  referred  to  as  an  open  kinematic  chain.  When  each  link 
is  kinematically  described  relative  to  its  previous  link,  a mathematical  kinematic  model  can  be  developed  to 
determine  the  position  and  orientation  of  the  last  link  with  respect  to  the  first  link  given  the  angular  position  of  each 
joint. 

1.4  Inverse  Kinematic  Model 

After  the  mathematical  kinematic  model  has  been  developed,  the  inverse  kinematic  model  can  be  extracted 
from  it.  The  inverse  kinematic  model  performs  the  opposite  operation  of  the  kinematic  model.  When  the  required 
position  and  orientation  of  the  end-effector  is  known,  the  inverse  kinematic  model  can  determine  the  required  angular 
position  for  each  joint  to  obtain  that  position  and  orientation. 


Product  Endorsement  Disclaimer 

Reference  to  specific  brands,  equipment,  or  trade  names  in  this  document  are  made  to  facilitate  understanding 
and  do  not  imply  endorsement  by  the  National  Institute  of  Standards  and  Technology. 
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1.5  Uses  for  Kinematic  Models 

The  kinematic  model  will  determine  the  position  and  orientation  of  the  last  frame  with  respect  to  the  first 
frame  given  the  angular  positions  of  each  of  the  joints.  However,  the  kinematic  model  is  primarily  used  to  develop 
the  inverse  kinematic  model  which  is  much  more  useful  in  robotic  programming.  If  the  robot  controller  has  a joint 
interface,  the  joint  angles  obtained  from  the  inverse  kinematic  model  are  fed  to  the  controller  to  have  the  robot  move 
to  a specified  position  and  orientation.  More  often  than  not,  the  end-effector  is  required  to  move  to  a specified 
position  and  orientation.  The  inverse  kinematic  model  will  determine  the  necessary  joint  angles  to  reach  the 
specified  goal,  and  each  joint  is  actuated  to  the  necessary  angle. 
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2 KINEMATIC  MODEL 


2.1  Kinematics 

Kinematics  is  the  science  of  motion  that  disregards  the  forces  that  cause  it.  The  study  of  the  kinematics  of 
manipulators  refers  to  the  geometrical  and  time-based  properties  of  the  motion. 

Manipulators  consist  of  rigid  links  which  are  connected  to  each  other  with  joints  that  allow  relative 
motion  of  the  neighboring  links.  Position  sensors  at  each  joint  measure  the  relative  position  of  neighboring  links. 

The  T^-646  is  a six  degree  of  freedom  manipulator.  The  number  of  degrees  of  freedom  that  a manipulator 
possesses  is  the  number  of  independent  position  variables  which  have  to  be  specified  in  order  to  locate  all  parts  of 

the  mechanism.  The  T^-646  has  six  revolute  joints,  and  therefore  six  degrees  of  freedom. 

The  kinematic  model  of  the  manipulator  is  a mathematical  model  that  computes  the  position  and 
orientation  of  the  end-effector  with  respect  to  the  base  frame  given  a set  of  joint  angles.  The  kinematic  model  for  the 
ADACS  robot  was  developed  using  the  standardized  Denavit-Hartenberg  notation. 

2.2  Denavit-Hartenberg  Notation 

The  Denavit-Hartenberg  notation  was  developed  as  a systematic  method  of  describing  the  kinematic 
relationship  between  a pair  of  adjacent  links  involved  in  an  open  kinematic  chain.  The  Denavit-Hartenberg  method 
is  based  on  a 4x4  matrix  representation  of  the  rigid  body  position  and  orientation.  A minimum  of  four  parameters 
are  necessary  to  completely  describe  the  kinematic  relationship  between  links. 

In  order  to  obtain  the  parameters  of  each  link,  and  therefore  describe  the  location  of  each  link  relative  to  its 
neighbors,  a frame  is  rigidly  attached  to  each  link. 


Joint  i - 1 


Figure  1:  The  Denavit-Hartenberg  Parameters 
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Figure  1 shows  a pair  of  adjacent  links,  link  i-1  and  link  i and  their  associated  joints,  i-1,  i,  and 
i+1.  The  convention  used  to  attach  the  frames  on  each  corresponding  link  is  as  follows: 

* The  origin  of  the  i-th  coordinate  frame  O is  located  at  the  intersection  of  joint  axis  i+1 
and  the  common  normal  between  joint  axes  i and  i+1,  as  shown  in  the  figure. 

* NOTE:  The  frame  of  link  i is  at  joint  i+1  rather  than  at  joint  i 

* The  X axis  is  directed  along  the  common  normal 

* The  Z axis  is  along  the  joint  axis  i+1 

* The  Y axis  is  chosen  to  form  a right-hand  coordinate  system 

The  relationship  between  the  two  frames  can  be  completely  described  using  the  following  parameters: 
aj  the  length  of  the  common  normal  (the  distance  from  Zj.^  to  Zj  as  measured  along  Xj.^ ) 
dj  the  distance  between  the  origin  Oj..|  and  the  point  Hj 
a j the  angle  between  the  joint  axis  i and  the  Zj  axis  in  the  right-hand  sense 


0j  the  angle  between  the  Xj..|  and  the  common  normal  HjOj  measured  about  the  Zj  axis  in 
the  right-hand  sense 

The  are  two  constant  parameters,  aj  and  Hj,  that  are  determined  by  the  geometry  of  the  robot  link.  One  of 
the  other  two  parameters  (Got  d)  varies  as  the  link  moves.  If  the  link  is  prismatic  (adjacent  links  translate  linearly 
to  each  other  along  the  joint  axis)  d will  be  the  variable.  If  the  link  has  a revolute  joint  (adjacent  links  rotate  with 

respect  to  each  other  along  the  joint  axis),  0 will  change  as  the  link  moves.  In  the  case  of  the  T^-  646  robot,  all  the 
joints  are  revolute,  therefore  a,  a and  d remain  constant  for  each  individual  link  while  0 changes  as  the  link  is  moved. 
Figure  2 shows  the  relationship  between  adjacent  coordinate  frames. 


Figure  2:  The  relationship  between  adjacent  coordinate  frames  in  the  Denavit-Hartenberg  notation 
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2.3  Denavit-Hartenberg  Frames  for  T^-646 

Using  the  Denavit-Hartenberg  notation,  frames  are  attached  to  each  link  of  the  robot.  After  the  links  are 
attached,  the  parameters  can  be  determined  for  each  link.  Figure  3 shows  the  assigned  frame  for  each  link  of  the 
robot  The  parameters  determined  from  these  frames  are  shown  in  Table  1. 


*3 


Figure  3:  Coordinate  frames  using  the  Denavit-Hartenberg  notation,  robot  is  shown  in  zero  or  home  position 
2.4  Denavit-Hartenberg  Parameters  for  T^-646 

Table  1 : Denavit-Hartenberg  parameters  for  the  ADACS  robot 


Link  Number 


0i 


a\ 


0 1 -<-180' 
02  -(-90° 
03 
-90° 

04  + 180° 

05 

06 


0 

0 

0 

1612.6 

0 

0 

0 


0 

1000 

200 

0 

0 

0 

0 


90° 

0 

-90° 

0 

-61  ° 
61° 
0 
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2.5  TVansformation  Matrixes  for  T^-646 

After  the  Denavit-Hartenberg  parameters  have  been  determined  for  each  link  of  the  robot,  a matrix  for  each 
link  can  be  constructed  to  represent  the  position  and  orientation  of  frame  i relative  to  frame  i - 1 . The  general  matrix 
is  shown  below. 


T|-’ 


cos 
sin  01 
0 
0 


-sin6|Cosai  sin  e,  sin  a,  a|Cos  0^ 
cose|Cosa|  -coseisina,  a,  sin  e, 
sin  a,  cos  a,  d , 

0 0 1 


Substitution  of  the  parameters  for  each  link  into  this  matrix  produces  a 4 x 4 matrix  for  each  link  of  the 
robot.  The  first  three  3 x 1 column  vectors  of  the  matrix  contain  the  direction  cosines  of  the  coordinate  axis  of 
frame  i,  while  the  last  3 x 1 column  vector  contains  the  position  of  the  origin  Oj.  The  matrix  for  each  link  of  the 

robot  is  shown  below. 


-cose,  0 

T«  - -sine,  0 

1 ' 

0 1 


-sine,  0 

cose,  0 

0 0 

0 1 


cos(e2+90)  -sin(eJ•^90) 
T’  = sin{ej+90)  cos(ej+90) 
0 0 


0 I 000cos(ej+90) 

0 1000sin|ej+90) 

1 0 


cosBj 
sin  63 


0 


-sine,  200  00503 

cosOj  200  sin  63 

0 0 

0 1 


0 


0 


1 0 0 


0 1 1612.6 
0 0 1 


NOTES: 

* cos(e+  180)  = -cos  0 

* sin  (0-)- 180)  = -sin  0 


-cose,  sine, cos (-61)  -sine, sin (-61)  0 

js  = -sine,  -cos 6, cos (-61)  cose,sin (-61)  0 

0 sin  (-61)  cos  (-61)  0 

0 0 0 1 


cose,  -sine, cos (61)  sin 6, sin (61)  0 

sine^  cose^  cos (61)  -cose, sin (61)  0 

0 sin  (61)  cos  (61)  0 

0 0 0 1 


•sinej 

cose, 
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To  ease  in  the  transformation  from  frame  2 to  3,  an  additional  frame  was  added  to  the  robot.  This  frame  is 
labeled  3 and  is  rigidly  attached  to  the  arm  of  the  robot.  The  transformation  from  frame  2 to  frame  3 takes  into 
account  the  rotation  of  the  arm  about  the  joint  axis  of  frame  2 and  the  translation  along  the  X axis  of  frame  2,  aj  in 

the  Denavit-Hartenberg  parameters.  The  transformation  from  frame  3 to  frame  3 is  a translation  along  the  Z axis  of 
frame  3 , this  is  dj  in  the  Denavit-Hartenberg  parameters. 

2.6  Kinematic  Model  for  T^-646 

After  the  matrices  have  been  determined  for  each  link,  we  wish  to  determine  the  relationship  between  the 
position  and  orientation  of  the  last  frame  with  respect  to  the  base  frame  of  the  robot.  The  manipulator  arm  consists 
of  n-t-1  links  from  the  base  to  the  tip  of  the  end-effector,  in  which  relative  position  and  orientation  of  adjacent  links 
are  represented  by  the  4 x 4 matrices  developed  using  the  Denavit-Hartenberg  parameters.  If  n consecutive  coordinate 
transformations  are  made  along  the  manipulator  serial  linkage,  we  can  derive  the  end-effector  location  and  orientation 

with  respect  to  the  base  frame.  In  the  case  of  the  T^-646,  there  are  6 revolute  joints  to  transform.  Therefore,  the 
following  equation  can  be  derived 


■r?  = T?T^i|T!Tir5-r| 


where  Is  is  a 4 x 4 matrix  representation  of  the  position  and  orientation  of  the  last  frame  with  respect  to  the  base 
frame.  This  equation  is  referred  to  as  the  kinematic  equation  of  the  manipulator  arm  and  governs  the  fundamental 
kinematic  behavior  of  the  arm. 


rii 

I'ZI 

^31 

0 


ri2  ri3 

•■22  '’23 

r32  '■33 

0 0 


Py 

Pz 


1 


rii 

'-21 


^31 


ri2 

''22 


r32 


Orientation  matrix  of  the  end-effector 
with  respect  to  the  base  coordinate  frame 


Position  of  the  end-effector 
with  respect  to  the  base 
coordinate  frame 


It  should  be  noted  that  there  are  several  exceptions  to  the  Denavit-Hartenberg  notation  rule.  For  the  base 
and  last  link,  there  is  no  common  normal  since  each  of  these  links  has  only  one  joint  axis.  Therefore,  the  coordinate 
frames  are  defined  as  follows.  For  the  base  link,  the  origin  of  the  coordinate  frame  can  be  chosen  arbitrarily  on  joint 
axis  1.  The  Zq  axis  must  be  parallel  to  the  joint  axis,  but  the  orientation  of  the  X and  Y axes  about  the  joint  is 
arbitrary.  For  the  last  link,  the  origin  of  the  coordinate  frame  can  be  chosen  at  any  convenient  point  of  the  end- 
effector,  however  the  X axis  must  intersect  the  last  joint  axis  at  a right  angle. 
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3 INVERSE  KINEMATIC  MODEL 


3.1  Inverse  Kinematics 

The  kinematic  model  derived  in  the  previous  section  describes  the  relationship  between  the  given  joint 
displacements  and  the  resultant  end-effector  position  and  orientation.  Finding  the  end-effector  position  and 
orientation  from  a given  set  of  joint  values  is  known  as  a direct  kinematic  problem.  Finding  the  joint  displacements 
for  a given  end-effector  position  and  orientation  is  known  as  an  inverse  kinematic  problem. 

Solving  the  inverse  kinematic  problem  provides  a model  which  allows  the  end-effector  motion  to  be 
described  in  terms  of  the  joint  value  motion.  This  is  necessary  for  a joint-angle  robot  controller  interface. 

When  solving  the  direct  kinematic  model,  there  is  one  unique  end-effector  position  and  orientation  for  a 
given  set  of  joint  angles.  The  inverse  kinematic  problem,  on  the  other  hand,  is  more  complex  because  multiple 
solutions  can  exist  for  a given  end-effector  position  and  orientation.  It  is  also  possible  that  no  solutions  exist  for  a 
particular  range  of  end-effector  locations.  Further,  since  the  inverse  kinematic  equations  consist  of  nonlinear 
simultaneous  equations  involving  many  trigonometric  functions,  a closed-form  solution  is  not  always  possible  to 
derive.  In  this  case  the  joint  displacements  are  calculated  using  numerical  methods.  Fortunately,  a closed  form 

solution  can  be  derived  for  the  T^-646. 


3.2  Geometric  Solution  for  Joints  1,  2 and  3 

As  stated  before,  the  T^-646  is  a six  degree  of  freedom  robot  with  a three  roll  wrist.  This  configuration 
allows  the  determination  of  the  first  three  joints  to  be  solved  using  a geometric  model. 


Figure  4:  Geomeiric  Solution  for  Joints  1 , 2,  and  3 
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Figure  4 shows  the  how  the  first  three  (1,2,  and  3)  joint  angles  can  be  determined  for  a given  position  of 
the  manipulator.  The  center  of  the  wrist  is  considered  to  be  the  end-effector,  the  last  section  can  be 
considered  part  of  the  tool  transformation. 


The  geometry  is  shown  again  in  Figure  5 without  the  outline  of  the  robot  to  simplify  the  drawing  for  the 
calculations  of  the  joint  values. 


Figure  5:  Geometric  solution  for  Joints  1, 2,  and  3 
3.2.1  Solution  for  Joint  1 

The  solution  for  joint  1 can  be  determined  by  examining  the  projection  of  the  manipulator  arm  in  the  x-y 
plane  (left  side  of  Figure  5).  The  length  of  projection  a can  be  determined  to  be: 

a = (Px^  + Py2)2 


whae  is  the  length  of  the  projection  in  the  x-plane  (or  the  x position  of  the  end-effector  relative  to  the  base 
coordinate  frame),  and  Py  is  the  length  of  the  projection  in  the  y-plane  (or  the  y position  of  the  end-effector  relative 
to  the  base  coordinated  frame).  0j  can  then  be  determined  using  the  inverse  tangent  function  as  follows: 

01  = Atan2  (px,  Py)  = Atan2  j^j 


3.2.2  Solution  for  Joint  2 

Examining  the  right  section  of  Figure  5 it  can  be  determined  that 

AnglA  + AngIB  + 02  = 90° 
therefore:  62  = 90°  - AngIB  - AnglA 

To  solve  this  equation,  AnglA  and  AngIB  must  be  determined.  AnglA  is  defined  as  the  angle  made  between  the  line 
segment  C and  the  x axis  of  the  base  coordinate  system.  Segment  C is  defined  as: 

C = (px^  + Py^  + Pz^)2 
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Where  Py  position  of  the  end-effector  in  the  x axis  of  the  base  coordinate  system,  Py  the  position  of  the  end-effector 
in  the  y axis  of  the  base  coordinate  system,  and  p^  is  the  position  the  end-effector  in  the  z axis  of  the  base  coordinate 
system.  AnglA  can  then  be  determined  using  the  inverse  tangent  function  and  is  derived  as  follows: 


AnglA  = Tan'^  ^ = Tan^/ ^ — ;j- 

^ UPx^  -t-  Py2)2^ 


AngIB  can  be  determined  using  the  Law  of  Cosines  on  triangle  bed.  Using  the  Law  of  Cosines  it  can  be  determined 
that 

-1-  - 2cd  Cos(AnglB) 

AngIB  = Cos'^  .Jb^  - - d^j 

thoefore:  ' 2cd  ' 

Once  AnglA  and  AngIB  have  been  determined,  02  is  calculated. 

02  = 90°  - AngIB  - AnglA 


3.2.3  Solution  for  Joint  3 

The  solution  for  joint  3 is  a little  more  complicated  than  the  solutions  for  joints  1 and  2.  Examine  the 
right  section  of  Figure  5.  It  can  be  seen  that  the  Offset  angle  is  always  a constant  and  can  be  evaluated  using  the 
inverse  tangent  function. 

Offset  = Tan'"'  1612.6  mm  ^ 82.930  ° 

200  mm 

It  can  also  be  seen  that  AnglE  is  always  the  sum  of  the  Offset  angle  and  03  (NOTE:  03  is  negative  in  the  right 
section  of  Figure  5).  Therefexe 

03  = AnglE  - Offset 

AnglE  is  defined  as  the  angle  between  line  segment  b and  the  extension  of  line  segment  d.  Upon  further  inspection 
it  can  be  determined  that  AnglE  and  AngIC  are  supplementary  angles,  therefore 

AnglE  = 180°  - AngIC 

This  leads  to  another  problem,  AngIC  must  be  solved  for.  Using  the  Law  of  Cosines  on  triangle  bed  again,  it  can 
be  determined  that 

= b^  + -2bd  Cos(AnglC) 

AngIC  = Cos'^  ./c^  - b^  - ] 

thCTefore:  ' 2bd  ' 

03  can  then  be  determined  using  the  formula 

03  = AnglE  - Offset 

Therefore  it  has  been  determined  that  the  angular  positions  for  the  first  three  joints  can  be  determined  based  entirely 
on  the  position  of  the  end-effector  in  the  x axis  of  the  base  coordinate  frame,  p^^,  the  position  of  the  end-effector  in 
the  y axis  of  the  base  coordinate  frame,  Py,  and  the  position  of  the  end-effector  in  the  z axis  of  the  base  coordinate 
frame,  p^. 
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3.3  Solution  for  Joints  4,  5,  and  6 

In  the  previous  section,  the  displacements  for  the  first  three  joints  were  solved  for  geometrically.  The 
derivation  of  the  last  three  joints  is  a much  more  involved  algebraic  and  trigonometric  problem.  There  exists  two 
specific  problems  to  be  dealt  with.  The  first  problem  is  the  presence  of  a singularity  point.  A singularity  point 
occurs  when  two  or  more  joint  axes  line  up  causing  an  infinite  number  of  possible  solutions  for  any  given 
orientation.  A singularity  point  occurs  in  this  manipulator  when  links  4 and  6 line  up,  or  when  the  value  of  joint  5 
is  zero.  There  is  also  an  ambiguity  in  the  wrist.  The  are  two  solutions  for  the  last  three  joints  for  a given 
orientation.  This  ambiguity  will  be  referred  to  as  “wrist-flip”  and  “wrist-no-flip.” 


3.3.1  Rotation  Matrixes 

In  determining  the  values  of  the  last  three  joints,  only  the  rotation  part  of  the  transformation  matrix  is 
necessary  (there  are  no  position  changes  in  the  last  three  links,  just  orientation  changes).  The  rotation  matrixes  for 
the  links  are  shown  below. 


NOTE:  From  here  to  the  end  of  the  paper,  the  term  (Gj  + 90)  will  be  abbreviated  as  G^  therefore,  when  Gz  occurs 
in  a matrix  or  an  equation,  the  value  (G2  + 90)  must  be  placed  there.  For  example,  cos  G2  MUST  be  replaced  with 
the  value  cos  (G2  + 90).  This  abbreviation  is  necessary  to  simplify  the  complex  equations  to  follow. 


R° 


R 


R 


R» 


-cose,  0 -sine, 

-sine,  0 cose, 

0 1 0 

cosoj  -slne^  0 

Sine  2 cose^  0 

0 0 1 


cosea  0 -sines 

sine 3 0 cos  63 

0 -1  0 

0 1 0 

-10  0 
0 0 1 


-cos 

sin  0^  cos  (-61 ) 

- sin  G,  sin  (-61 ) 

- sin 

- cos  cos  (-61 ) 

cos  sin  (-61) 

0 

sin  (-61 ) 

cos  (-61 ) 

cos  Gg 

- sin  ^ cos  (61) 

sin  05  sin  (61) 

sinGg 

cos  63  cos  (61) 

- cos  0g  sin  (61) 

0 

sin  (61) 

cos  (61 ) 

COSGg 

-sin  Gg 

" 1 

sin 

cos  Gg 

0 

0 

1 J 

Once  the  values  are  known  for  joints  1,  2,  and  3,  these  values  can  be  substituted  back  into  the  kinematic 
model  to  derive  the  last  three  joint  values.  Looking  back  at  the  rotation  section  of  the  kinematic  model,  it  can  be 
seen  that  the  orientation  of  the  last  frame  with  respiect  to  the  base  frame  is  Re  = R2  Ra’  R4  Rs  Re.  If  we 
substitute  the  values  of  Gi,  G2,  and  Gaback  into  Ri  R2  RI  Ra,  this  rotation  matrix  gives  the  orientation  of  frame 
3 with  respect  to  the  base  frame  as  a numerical  matrix.  If  this  numerical  matrix  is  inverted  and  premultiplied  to  Re, 
we  can  obtain  the  numerical  rotation  matrix  of  the  last  frame  with  respect  to  frame  3,  Re.  The  symbolic  rotation 
matrix  of  the  last  frame  with  respect  to  frame  3 can  be  obtained  by  multiplying  the  rotation  matrixes  of  the  the  last 
three  links.  Re  = Ra  Rs  Re . We  now  have  the  symbolic  and  numerical  rotation  mauix  of  the  last  frame  with 
respect  to  frame  3.  The  equations  are  shown  again  below. 

Re  = Rs  Re  Symbolically 

Re  = (R°  R2  R|  Ra)  Re  Numerically 


12 


When  the  rotation  matrixes  are  multiplied  together  the  following  is  obtained.  NOTE:  Due  to  the 
complexity  of  the  following  calculations,  it  is  recommended  that  the  matrix  multiplication  be  performed  with  a 
software  package  such  as  Mathematica.  The  terms  COS  and  sin  have  been  abbreviated  to  C and  S respectively. 


F^RtRl 

Re  = (Ri  R2  RI  R!)'^  Re 

•’ll  •’12  ''13 

a b c 

hi  *^22  Tjs 

^6  = 

d e f 

tai  '’32  i'33 

g h i 

Symbolically  Numerically 


rii  = C6(-C4C5  + c(-61)S4S5)  + S6(C5C(-61)c(61)S4  + C4C(61)S5  - S4S(61)s(-61)  = a 
>■12  = S6(C4C5  - c(-61)S4S5)  + C6(C5C(61)c(-61)S4  + C4C(61)S5  - S4S(61)s(-61))  = b 
ri3=  -C5S4C(-61)s(61)  - C4S5S(61)  - c(61)s(-61)S4  =c 

f2i  = Cei-CsS^  - C4S5C(-61))  + S6(-C4C5C(61)c(-61)  + S4S5C(61)  + C4S(61)  s(-61))  = d 

r22  = S6(C5S4  + C4S5C(-61))  + C6(S4S5C(61)  - C4C5C(61)c(-61)  + C4S(61)s{-61))  = e 

^23  = C4C5C(-61)S(61)  - S4S5S(61)  + C4C(61)S(-61)  = f 

r3i  = C6S5S(-61)  + S6(c(-61)s(61)  + C5C{61)s(-61))  = g 

'’32  = C6(C(-61)S(61)  +C5C(61)S(-61))  - S5S6S(-61)  = h 

r33  = c(61)c(-€1)  - C5S(61)s(-61)  = i 

3.3.2  Singularity  Point 

As  stated  before,  a singularity  occurs  in  the  manipulator  when  65  is  equal  to  0.  When  the  above  rotation 
matrix  is  examined,  it  can  be  seen  that  the  r33  value  is  only  dependent  on  the  value  of  65. 

r33  = c(61)c(-61)  - C5S(61)s(-61)  = i 

Therefore,  we  must  determine  the  value  of  i when  85  is  equal  to  0.  The  value  of  Cos(O)  is  1 , therefore,  the 
manipulator  is  in  a singular  position  when 

r33  = c(61)c(-61)  - s(61)s(-61) 
r33  = 0.23504  - (-0.76496)  = 1 = i 

Therefore,  a singularity  occurs  when  the  value  of  i,  in  the  numerical  rotation  matrix,  is  equal  to  1 . When  a 
singularity  occurs,  the  value  of  85  is  set  to  0,  the  value  of  84  is  set  to  its  previous  value,  and  joint  6 is  determined 
and  actuated  to  the  proper  orientation.  NOTE:  In  the  control  program,  the  value  i must  be  checked  before  each 
calculation  to  determine  if  a singularity  occurs.  If  a singularity  occurs  and  the  value  of  84  is  not  set  to  its  previous 
position,  the  inverse  kinematic  model  will  explode. 
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3.3.3  Solution  for  Joint  6 


The  most  complex  part  of  the  solution  for  the  last  three  joints  is  determining  the  value  of  one  of  the  three 
joints.  This  is  accomplished  by  examining  the  symbolic  rotation  matrix  for  equations  that  can  be  used  to  eliminate 
all  the  variables  except  the  one  you  are  looking  for  and  to  combine  the  remaining  variables  into  a tangent  function. 
There  is  no  real  method  to  go  about  this  except  experience  in  knowing  what  to  look  for  and  trial  and  error. 

The  first  step  to  help  clean  up  the  matrix  components  is  to  evaluate  all  the  cosines  and  sines  of  61  and 
-61  degrees  found  in  the  equations.  These  cleaned  up  equations  are  shown  below. 

Til  = C6(-C4C5  + 0.48481  S4S5)  + Sei  0.23504C5  S4  + 0.48481C4  S5  + 0.76496S4)  = a 

ri2  = C6(0.23564C5  S4  + O.4848IC4S5+  O.76496S4) -H  Se ( -C4 C5  + O.4848IS4S5)  =b 

ri3  = 0.424024s4(1 -C5)  - O.87462C4S5  =C 

r2i  = C6(-C5S4  - C4S5C(-61))  + S6(-C4C5C(61)c(-61)  -I-  S4S5C(61)  + C4S(61)  s(-61))  = d 

^22  = S6{C5S4  + 0.4848IC4S5)  + C6(  0.48481  S4S5  - 0.23504C4C5  - 0.76496C4)  = e 

r23  = 0.424024C4(C5 -1)  - O.87462S4S5  =f 

r3i  = -0.87462C6S5  + 0.424024S6(1  -C5)  = g 

r32  = 0.424024C6(1 -C5)  + 0.87462S5S6  = h 

r33  = 0.23504  -I-  0.76496C5  = i 

Compare  the  g and  h terms  in  each  matrix 

r3i  = -0.87462C6S5  + 0.424024S6(1  - C5)  = g 
r32  = 0.87462S5S6  + 0.424024C6(1 -C5)  =h 

Multiply  the  top  equation  by  Se 
Multiply  the  bottom  equation  by  Ce 
Then  add  the  equations 

Seg  = -0.87462C6S5  Sg  -t-  0.424024S6  2 (1  -C5) 

Cg  h = 0.87462S5  Sg  Cg  + 0.424024Cg  2 (1  - Cg ) 


Sg  g -H  Cg  h = 0.424024(1  - C5)(Sg  2 . Sg  2 . Cg2  = 1 

Sgg  + Cg  h = 0.424024(1  -Cg)  Eq.  1 

Wfe  can  now  make  a substitution  for  Cg 
Compare  the  i terms  in  each  matrix 


Therefore 


i = 0.23504  + 0.76496  Cg 
Cg  = (i  - 0.23504)/0.76496 
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Substituting  this  into  Eq.  1,  we  obtain  the  following 


Sgg  + Cgh  = 0.424024(1  -(i  - 0.23504)/0.76496) 
To  continue,  some  trigonometric  substitutions  must  be  employed. 


1 - Tan^^ 

Cose  = 

1 +Tan^-6- 
2 


Sin 


2Tan.0- 
2_ 

1 +Tan^-Q- 
2 


And  to  ease  in  the  notation,  the  following  substitution  is  made 

Tan  1 = K Tan  ^ = K6 

2 2 

Making  these  substitutions  into  Eq.  2,  we  obtain 

2J<6_jg  / 1 - KeiU  = 0.424024  (l  - 1--.  0.23504 

' 0.76496 


1 + VI  + Ke 


Substituting  n for  the  right  side  of  Eq.  3 


n = .424024  1 - i - Q-235Q4 
V 0.76496  I 


obtain 


2 Ke 


1 - Ke*" 


h = n 


ll  + Ke^/  \1  + Ke" 
Multiplying  both  sides  of  the  equation  by  (1  + Ke  2) 


2K6g  + (1  - K6^)h  = n(1  H-Ke^) 


Multiplying  the  h and  n through  the  parentheses 

2K6g+  h - hKe^  = n + nK6^ 

Rearranging  Eq.  4 

(h  + n)  Ke^  - (2  g)  Ke  + (n  - h)  = 0 

Upon  examining  Eq.  5,  it  can  be  seen  that  it  is  a quadratic  equation  of  the  form 

ax^  + bx  + c = 0 

WhCTe:  a = (h  + n) 

b = -2g 
c = (n  - h) 

X = Ke 

The  roots  of  a quadratic  equation  are 


-b  ± Vb^  - 4ac 


2a 


Eq.  2 


Eq.  3 


Eq.  4 

Eq.  5 
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Making  the  substitutions  into  this  equation  and  solving  for  the  roots,  the  following  is  obtained 

g ± Vg^  + 

(h  + n) 


Eq.  6 


Therefore,  from  Eq.  6,  the  roots  are 

K.  _ g + Vg"  -H  K6,=SjJE±ZZZ 

’ (h  + n)  (h  + n) 

Remembering  the  substitution 

Tan^  = K6 
2 

Therefwe  06  = 2 Tan  ’ Ke 

Because  there  are  two  possible  roots,  there  are  two  possible  solutions  for  06 . 

06,  = 2 Tan"'  K6i 
062  = 2 Tan  ’ Keg 

Making  the  final  substitution  for  K back  into  these  equations,  the  two  solutions  for  0$  are  obtained. 

66,  = 2Tan-'(9J-!!ZIZIZ)  = 2Tan-’(o.jEIZZZ| 

\ (h  + n)  / \ (h  + n)  / 

The  two  possible  solutions  for  06  causes  the  ambiguity  in  the  wrist  There  are  two  possible  solution  sets 
for  {04,  05,  06}  that  give  the  required  orientation  for  the  end-effector.  These  sets  are  found  by  backing  out  05  and  04 
using  both  solutions  for  06-  A set  is  determined  using  the  first  solution  of  06  and  a second  set  is  determined  using 
the  second  solution  of  06.  The  set  that  requires  the  least  amount  of  movement  is  then  sent  to  the  controller. 


3.3.4  Solution  for  Joint  5 

After  06  has  been  determined,  its  value  is  substituted  back  into  its  rotation  matrix. 

tcos  06  -sin  06  ° I 

sin  06  cos  06  ° 

0 0 1 J 

Therefore,  this  matrix  has  a numerical  value.  This  matrix  is  then  inverted  and  post  multiplied  to  the  rotation  matrix 
that  gives  the  orientation  of  the  last  frame  with  respect  to  the  third  frame. 

Re  Re  ^ = Re  RI  = Numerically 

Rs  = R4R5  Symbolically 
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Ri  = 


Rs  = r1  = rI  rI 


Til 

>■12 

^3 

a 

b 

^ 1 

>'22 

'-23 

^5  = 

d 

e 

'’32 

^33 

g 

h 

' J 

Symbolically  Numerically 


= -C4  Cs  + 0.48481  S4  S5  = a 
ri2  = 0.76496  S4  + 0.23504  C5S4  + 0.48481  C4S5  = b 
ri3  = 0.424024  84(1  -Cg)  - 0.87462  C4S5  = C 
^21  = -S4C5  - 0.48481  C4S5  = d 
r22  = -0.76496  C4  - 0.23504  C4C5  + 0.48481  8485=  e 
r23  = 0.424024  C4  (C5  - 1)  - 0.87462  84  Sg  = f 
r3i  = -0.87462  8g  = g 
r32  = 0.424024  - 0.424024  Cg  = h 
r33  = 0.23504  + 0.76496  Cg  = i 


Comparing  the  g and  h terms  of  each  matrix,  it  can  be  seen  that  a tangent  function  can  be  formed  rather  easily. 

Tan  05  = 

CO8  05 

I31  “ -0.87462  Sg  = g 

r32  = 0.424024  - 0.424024  Cg  = h 


Solving  for  8g  and  Cg  we  obtain 


Sin  05  = - ( 3 

10.87462 


C0S65  = -(t?-:  ■0-424024) 
1 0.424024  I 


Combining  the  Sin  and  C08  functions,  Og  can  be  solved. 


05  = Tan' 


-g 

0.87462 

(h  - 0,424024 
\ ' 0.424024 
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There  are  two  solutions  for  6g.  One  is  determined  using  the  first  solution  for  Og,  and  a second  is  determined 
using  the  second  solution  for  Og.  These  solutions  MUST  be  kept  in  their  respective  sets.  DO  NOT  combine  them 
into  one  set  or  mix  the  sets.  Incorrect  joint  angles  will  be  calculated  if  this  is  not  followed. 
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3.3.5  Solution  for  Joint  4 


After  the  value  for  85  has  been  calculated,  it  is  substituted  back  into  its  rotation  matrix. 


R 


- sin  % cos  (61 ) 
cos  Og  cos  (61 ) 
sin  (61) 


sin  Os  sio  (61) 
cosOsSio  (61) 
cos  (61) 


] 


Therefore,  this  matrix  has  a numerical  value.  This  matrix  is  then  inverted  and  post  multiplied  to  the  rotation  matrix 
that  gives  the  orientation  of  the  fifth  frame  with  respect  to  the  third  frame. 


R4  = Rs  Rs  = Rs  R4  Numerically 

R4  = R4  Symbolically 


Symbolically  Numerically 

rii  = -C4  =a 
ri2  = c(-61)  S4  = b 
ri3  = -s(-61)S4  = c 
r2i  = -S4  = d 

r22  = -c(-61)  C4  = e 
r23  = s(-61)  C4  = f 

rai  = 0 

h2  = s{-61) 
r33  =c(-61) 

Upon  inspection  of  the  rotation  matrix,  it  can  be  seen  that  a tangent  function  can  easily  be  created  by  using 
the  a and  d terms  of  each  matrix. 

rii  = -C4  =a 
<'21  = -S4  = d 

Tan  04  = 

Cos  04 

e4=Tar'(^) 

Once  again,  there  are  two  solutions  for  84.  One  is  determined  using  the  first  solution  for  85,  and  a second  is 
determined  using  the  second  solution  for  06.  These  solutions  MUST  be  kept  in  their  respective  sets.  DO  NOT 
combine  them  into  one  set  or  mix  the  sets.  Incorrect  joint  angles  will  be  calculated  if  this  is  not  followed. 

After  04  has  been  calculated  for  each  solution  for  06,  there  are  two  complete  sets  of  joint  angles  for  the 
given  position  and  orientation.  0i  , 02 , and  83  will  be  the  same  for  each  set.  The  two  sets,  labeled  “wrist-flip”  and 

“wrist-no-flip”,  are  then  compared  with  the  previous  angles  of  the  manipulator  and  the  set  with  the  closest  to  this  is 
sent  to  the  robot  controller  and  the  joints  are  actuated  to  the  calculated  value. 
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4 ALTERNATIVE  KINEMATIC  MODEL 


4.1  Link  lYansform  Approach 

The  link  transform  approach  assigns  a coordinate  frame  to  each  link  of  the  robot  The  coordinate  frame  is 
placed  at  the  joint  of  the  link  with  the  joint  revolving  around  the  Z axis  in  a right-hand  sense.  After  a coordinate 
frame  has  been  assigned  to  the  base  of  the  robot  and  to  each  link  of  the  robot  a kinematic  equation  can  be  developed 
to  relate  the  position  and  orientation  of  the  last  frame  (the  end-effector)  to  the  base  frame.  Figure  6 below  shows 
the  frames  that  were  assigned  to  the  robot  for  the  link  transform  approach. 


Figure  6:  Frames  assigned  to  each  link  using  the  Link  Transform  Approach,  robot  shown  in  zero  or  home  position 

A transformation  matrix  can  be  calculated  for  each  link  by  comparing  the  relative  motion  of  a frame  with 
respect  to  its  previous  frame.  As  a joint  is  rotated,  the  frame  of  the  corresponding  link  also  moves  by  a certain 
amount  relative  to  the  previous  frame.  An  example  of  this  is  shown  in  Figure  7. 


Zi  Zo 


Figure  7:  Relative  motion  of  frame  1 with  respect  to  frame  0 
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From  Figure  7,  the  transformation  matrix  that  relates  the  motion  of  frame  1 with  respect  to  frame  0 can  be 
derived.  The  following  is  how  this  transformation  matrix  is  derived.  Als  link  1 (and  frame  1)  revolves  with  respect 
to  link  0 (and  frame  0)  in  a positive  direction,  the  following  happens: 

The  Xi  axis  varies  from  1 @ 0 degrees  to  0 (S)  90  degrees  on  the  Xq  axis.  This  motion  is 
defined  by  the  cosine  of  joint  angle  1. 

The  Xi  axis  varies  from  0 (S)  0 degrees  to  1 @ 90  degrees  on  the  yo  axis.  This  motion  is 
defined  by  the  sine  of  joint  angle  1. 

The  Xi  axis  is  always  0 on  the  Zq  axis  because  it  always  remains  perpendicular  to  it. 

The  Vi  axis  varies  from  0 @ 0 degrees  to  -1  (5)  90  degrees  on  the  x©  axis.  This  motion  is 
defined  by  the  -sine  of  joint  angle  1 . 

The  yi  axis  varies  from  1 @ 0 degrees  to  0 @ 90  degrees  on  the  yo  axis.  This  motion  is 
defined  by  the  cosine  of  joint  angle  1. 

The  yi  axis  is  always  0 on  thezo  axis  because  of  its  perpendicularity. 

The  Zi  axis  remains  coincident  with  the  Zo  axis  during  all  motion. 

Px  = Distance  along  the  Xj.i  axis  from  frame  i-1  to  frame  i = 0 

Py  = Distance  along  the  yj.^  axis  from  frame  i-1  to  frame  i = 0 

Pz  = Distance  along  the  Zj.^  axis  from  frame  i-1  to  frame  i = 0 

From  this  information,  a transform  matrix  can  be  assembled.  This  is  a 4 x 4 matrix  with  the  bottom  row  comprised 
of  fillers  {0, 0, 0, 1 ) that  do  not  effect  the  output  of  the  matrix. 


= ' M 

' Zm 


Filling  in  this  generic  matrix,  the  transformation  matrix  for  the  first  joint  (i  = 1)  is  obtained. 
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4.2  lYansformation  Matrixes 


The  preceding  steps  are  performed  for  each  joint  and  the  following  transform  matrices  are  obtained. 
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4.3  Kinematic  Model  for  T^-646 

Once  the  link  transform  matrix  for  each  link  has  been  calculated,  the  kinematic  equations  can  be  solved  for 
and  the  kinematic  model  obtained.  This  is  performed  in  the  same  manner  that  the  kinematic  model  was  developed 
using  the  Denavit-Hartenberg  parameters 


T6=T?T2'l3'T3T4T6T6 


where  le  is  a 4 x 4 matrix  representation  of  the  position  and  orientation  of  the  last  frame  with  respect  to  the  base 
frame.  This  equation  is  referred  to  as  the  kinematic  equation  of  the  manipulator  arm  and  governs  the  fundamental 
kinematic  behavior  of  the  arm. 
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Orientation  matrix  of  the  end-effector 
with  respect  to  the  base  coordinate  frame 


Position  of  the  end-effector 
with  respect  to  the  base 
coordinate  frame 


NOTE:  Although  this  method  provides  a kinematic  model  that  is  correct,  the  Denavit-Hartenberg  method  is 
preferred  due  to  its  standard  frame  placements  that  can  be  easily  duplicated. 
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5.  CONCLUSION 


The  principles  used  to  derive  the  forward  and  inverse  kinematics  for  the  T^-646  can  be  applied  to  most 
modem  “open  loop”  industrial  robots.  The  Denavit-Hartenberg  parameters  provide  a standard  method  of  frame 
attachment  for  each  joint  of  the  robot.  The  forward  kinematic  model  provides  a mathematical  model  that  describes 
the  position  and  orientation  of  the  last  frame  with  respect  to  the  base  frame  given  a set  of  joint  angles.  The  inverse 
kinematic  model,  derived  from  the  forward  kinematic  model,  provides  a mathematical  model  that  determines  the 
required  joint  positions  for  a given  end-effector  position  and  orientation.  Using  the  derived  inverse  kinematic  model, 
the  joint  angles  can  be  determined  for  each  position  and  orientation  of  the  end-effector,  making  a joint  interface 
possible. 
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